/*
 * Docking.cpp
 *
 *  Created on: 2017年5月25日
 *      Author: roswiki
 */

#include <katrina_gui/Docking.h>

namespace Katrina
{
    Docker::Docker() :
            rate_(4), has_ir_(false), asp_(2), anti_clock_(true)
    {
        ir_state_sub_ = nh_.subscribe("/ir_state", 10, &Docker::irStateCallback, this);
        start_sub_ = nh_.subscribe("/start_docking", 10, &Docker::startCallback, this);
        cmd_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 10, true);
        finish_pub_ = nh_.advertise<std_msgs::Bool>("/finish_docking", 10, true);

        linear_vel_ = -0.08;
        angular_vel_ = 0.25;
        asp_.start();
        ROS_INFO("Docking is ready.");
    }

    Docker::~Docker()
    {

    }

    void Docker::irStateCallback(const std_msgs::UInt32MultiArrayConstPtr &msg)
    {
        if (!has_ir_)
            has_ir_ = true;
        ir_state_.resize(msg->data.size());
        ir_state_ = msg->data;
//        ROS_INFO_STREAM("Get data "<<ir_state_[0]<<","<<ir_state_[1]<<","<<ir_state_[2]<<","<<ir_state_[3]);
    }

    void Docker::startCallback(const std_msgs::BoolConstPtr &msg)
    {
        ROS_INFO("Start docking");
        docking();
    }

    void Docker::docking()
    {
        ros::Time start = ros::Time::now();

        while (!has_ir_ && ros::Duration(ros::Time::now() - start).toSec() > 3)
        {
            ros::Duration(0.1).sleep();
        }
        if (!has_ir_)
        {
            ROS_ERROR("No IR is received");
            return;
        }
//        ROS_INFO_STREAM("Process data "<<ir_state_[0]<<","<<ir_state_[1]<<","<<ir_state_[2]<<","<<ir_state_[3]);
        while (!ir_state_[3])
        {
            geometry_msgs::Twist move_cmd;
            if (ir_state_[0] && ir_state_[1] && ir_state_[2])
            {
                move_cmd.linear.x = linear_vel_;
                move_cmd.linear.y = 0;
                move_cmd.angular.z = 0;
                ROS_INFO("Send 1");
            }
            else if (ir_state_[0] && !ir_state_[1] && ir_state_[2])
            {
                move_cmd.linear.x = linear_vel_;
                move_cmd.linear.y = 0;
                move_cmd.angular.z = 0;
                ROS_INFO("Send 2");
            }
            else if (!ir_state_[0] && ir_state_[1] && !ir_state_[2])
            {
                move_cmd.linear.x = linear_vel_;
                move_cmd.linear.y = 0;
                move_cmd.angular.z = 0;
                ROS_INFO("Send 3");
            }
            else if (ir_state_[0] && ir_state_[1] && !ir_state_[2])
            {
                move_cmd.linear.x = linear_vel_ / 2.0;
                move_cmd.linear.y = 0;
                move_cmd.angular.z = -angular_vel_ / 2.0;
                ROS_INFO("Send 4");
            }
            else if (ir_state_[0] && !ir_state_[1] && !ir_state_[2])
            {
                move_cmd.linear.x = 0;
                move_cmd.linear.y = 0;
                move_cmd.angular.z = -angular_vel_ / 2.0;
                ROS_INFO("Send 5");
            }
            else if (!ir_state_[0] && ir_state_[1] && ir_state_[2])
            {
                move_cmd.linear.x = linear_vel_ / 2.0;
                move_cmd.linear.y = 0;
                move_cmd.angular.z = angular_vel_ / 2.0;
                ROS_INFO("Send 6");
            }
            else if (!ir_state_[0] && !ir_state_[1] && ir_state_[2])
            {
                move_cmd.linear.x = 0;
                move_cmd.linear.y = 0;
                move_cmd.angular.z = angular_vel_ / 2.0;
                ROS_INFO("Send 7");
            }
            else if (!ir_state_[0] && !ir_state_[1] && !ir_state_[2])
            {
                move_cmd.linear.x = 0;
                move_cmd.linear.y = 0;
                move_cmd.angular.z = 0.2; /*(anti_clock_ ? -1 : 1) * 0.2;*/

                anti_clock_ = !anti_clock_;
                ROS_INFO("Send 8");
            }
            cmd_pub_.publish(move_cmd);
            rate_.sleep();
        }
        done();
    }

    void Docker::done()
    {
        ROS_ERROR("Found base!");
        geometry_msgs::Twist move_cmd;
        move_cmd.linear.x = move_cmd.linear.y = move_cmd.angular.z = 0;
        cmd_pub_.publish(move_cmd);
        std_msgs::Bool done;
        done.data = true;
        finish_pub_.publish(done);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "KatrinaDocker");
    ros::NodeHandle n;
    Katrina::Docker myDocer;
    ros::waitForShutdown();
    return 0;
}
